//
// Created by 李家豪 on 2019/6/5.
//

#include "timer.h"
#include "math_.h"
#include "types.h"
#include "stdint.h"
#include "ReferenceFrameSolver.h"

frame_t frame;
extern att_t att;
extern imu_t imu;

/**
 * Capture current frame as the reference frame.
 *
 * This function will ignore subsequent calls until
 * the `frame_reset` is called.
 */
void frame_capture() {
	if (frame.captured)
		return;
	frame.angleDelta = 0;
	frame.currentAngle = 0;
	frame.capturedAngle16 = att.heading;
	frame.capturedAngle = ((float) att.heading) * DEG_TO_RAD;
	frame.captured = 1;
	frame.lastSamplingTime = micros();
}

/**
 * Clear the capture flag.
 *
 * This will not affect any other value.
 */
void frame_reset() {
	frame.captured = 0;
}

/**
 * Sample current orientation and solve for angle differences.
 *
 * The frame must be captured first with `frame_capture`.
 *
 * This function will not do any conversion and should be called
 * before any conversion calculation.
 */
void frame_sample() {
	if (!frame.captured)
		return;
	frame.angleDelta16 = att.heading - frame.capturedAngle16;
	frame.currentAngle = ((float) att.heading) * DEG_TO_RAD;
	frame.angleDelta = ((float) frame.angleDelta16) * DEG_TO_RAD;
	frame.pitchDelta = ((float) att.angle[PITCH]) / 10.0 * DEG_TO_RAD;
	frame.rollDelta = ((float) att.angle[ROLL]) / 10.0 * DEG_TO_RAD;
	frame.currentSamplingTime = micros();
}

/**
 * Convert the referenced coordinate to base coordinate.
 *
 * Rotation Matrix:
 * <pre>
 * / cos(theta)  sin(theta) \<br>
 * \ -sin(theta) cos(theta) /
 * </pre>
 *
 * @param x IO, the X coordinate
 * @param y IO, the Y coordinate
 */
void frame_2d16_from_reference(int16_t *x, int16_t *y) {
	float cosTheta = fcos(frame.angleDelta);
	float sinTheta = fsin(frame.angleDelta);
	int16_t newX, newY;
	newX = *x * cosTheta + *y * sinTheta;
	newY = *y * cosTheta - *x * sinTheta;
	*x = newX;
	*y = newY;
}

/**
 * Project base vector to referenced vector.
 *
 * @param x IO, the X component
 * @param y IO, the Y component
 */
void frame_2d16_project_to_reference(int16_t *x, int16_t *y) {

}

/**
 * Convert the base coordinate to reference coordinate.
 *
 * Rotation matrix:
 * <pre>
 * / cos(theta) -sin(theta) \<br>
 * \ sin(theta)  cos(theta) /
 * </pre>
 *
 * @param x IO, the X coordinate
 * @param y IO, the Y coordinate
 */
void frame_2d16_from_base(int16_t *x, int16_t *y) {
	float cosTheta = fcos(frame.angleDelta);
	float sinTheta = fsin(frame.angleDelta);
	int16_t newX, newY;
	newX = *x * cosTheta - *y * sinTheta;
	newY = *y * cosTheta + *x * sinTheta;
	*x = newX;
	*y = newY;
}

/**
 * Project reference vector to base vector
 * @param x
 * @param y
 */
void frame_2d16_project_to_base(int16_t *x, int16_t *y) {

}

/**
 * Integrate the gyro.
 *
 * Should be called every movement frame.
 */
void frame_offset_calculate() {
	if (!frame.captured)
		return;
	uint16_t dT = frame.currentSamplingTime - frame.lastSamplingTime; // ms
	// ^ if this overflows, it means you have a big trouble
	int32_t dYaw = dT * imu.gyroData[YAW];
	int32_t dPitch = dT * imu.gyroData[PITCH];
	int32_t dRoll = dT * imu.gyroData[ROLL];
	// TODO: figure out the X, Y, Z of the sensor
	frame.lastSamplingTime = frame.currentSamplingTime;
}

/**
 * Make adjustment to gyro integral.
 *
 * Should be called once every a while, like per 5 seconds.
 *
 * This function will fetch and merge other sensors' data, like
 * optic movement sensor for X-Y plane and barometer for Z plane.
 */
void frame_offset_adjustment() {
	// TODO
}
